/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package teamBlitz;

import com.sun.squawk.security.HexEncoding;
import edu.wpi.first.wpilibj.*;
import java.util.TimerTask;

/**
 *
 * @author dtjones
 */
public class ADXL345_I2C_6DOF extends I2CSync {

    private static final byte kAddress = (byte) 0xa6;
    private static final byte kPowerCtlRegister = 0x2D;
    private static final byte OFSX = 0x1e;
    private static final byte OFSY = 0x1f;
    private static final byte OFSZ = 0x20;
    private static final byte kIntEnable = 0x2e;
    private static final byte kIntMap = 0x2f;
    private static final byte kIntSource = 0x30;
    private static final byte kDataFormatRegister = 0x31;
    private static final byte kDataRegister = 0x32;
    private static final byte kFIFOCntl = 0x38;
    private static final byte kFIFOStatus = 0x39;
    private static final double kGsPerLSB = 0.00390625;
    private static final byte kPowerCtl_Link = 0x20, kPowerCtl_AutoSleep = 0x10, kPowerCtl_Measure = 0x08, kPowerCtl_Sleep = 0x04;
    private static final byte kDataFormat_SelfTest = (byte) 0x80, kDataFormat_SPI = 0x40, kDataFormat_IntInvert = 0x20, kDataFormat_FullRes = 0x08, kDataFormat_Justify = 0x04;
    static final private int SAMPLE_TIME_ms = 10;  // sample time in ms
    private boolean m_Calibrate = false;
    private boolean mUseZ = false;
    private static int ActCount=4;
    private static int InActCount=2;

    public static class DataFormat_Range {

        /**
         * The integer value representing this enumeration
         */
        public final byte value;
        public final float gValue;
        static final byte k2G_val = 0x00;
        static final byte k4G_val = 0x01;
        static final byte k8G_val = 0x02;
        static final byte k16G_val = 0x03;
        public static final DataFormat_Range k2G = new DataFormat_Range(k2G_val, 2);
        public static final DataFormat_Range k4G = new DataFormat_Range(k4G_val, 4);
        public static final DataFormat_Range k8G = new DataFormat_Range(k8G_val, 8);
        public static final DataFormat_Range k16G = new DataFormat_Range(k16G_val, 16);

        private DataFormat_Range(byte value, float g) {
            this.value = value;
            this.gValue = g;
        }
    }

    public static class Axes {

        /**
         * The integer value representing this enumeration
         */
        public final byte value;
        static final byte kX_val = 0x00;
        static final byte kY_val = 0x02;
        static final byte kZ_val = 0x04;
        public static final Axes kX = new Axes(kX_val);
        public static final Axes kY = new Axes(kY_val);
        public static final Axes kZ = new Axes(kZ_val);

        private Axes(byte value) {
            this.value = value;
        }
    }

    public static class AllAxes {

        public double XAxis;
        public double YAxis;
        public double ZAxis;

        public void clear() {
            XAxis = 0;
            YAxis = 0;
            ZAxis = 0;
        }

        public void set(double x, double y, double z) {
            XAxis = x;
            YAxis = y;
            ZAxis = z;
        }

        public void add(double x, double y, double z) {
            XAxis += x;
            YAxis += y;
            ZAxis += z;
        }

        public void add(AllAxes in) {
            XAxis += in.XAxis;
            YAxis += in.YAxis;
            ZAxis += in.ZAxis;
        }

        public void subtract(AllAxes in) {
            XAxis -= in.XAxis;
            YAxis -= in.YAxis;
            ZAxis -= in.ZAxis;
        }
    }
    AllAxes accData;
    private I2C m_i2c;
    int mCount = 0;
    ADXL345_I2C_6DOF.AllAxes accTotal;
    ADXL345_I2C_6DOF.AllAxes accStill;
    ADXL345_I2C_6DOF.AllAxes accMax;
    private java.util.Timer m_controlLoop;
    boolean mActive = false;
    boolean mActivityX = false;
    boolean mActivityY = false;
    boolean mActivityZ = false;
    int mActivityCountX = 0;
    int mActivityCountY = 0;
    int mActivityCountZ = 0;

    public boolean isActive() {
        return mActive;
    }

    public void setActive(boolean mActive) {
        accTotal.clear();
        mCount = 0;
        this.mActive = mActive;
    }

    public ADXL345_I2C_6DOF(int slot, int InterruptInput, DataFormat_Range range) throws InstantiationException {
        super(DigitalModule.getInstance(slot), kAddress);
        ////System.out.println("Starting accelerometer");


        boolean rc = false;
        accData = new AllAxes();
        accStill = new AllAxes();
        accMax = new AllAxes();
        accTotal = new AllAxes();

        // Turn on the measurements
        rc = write(kPowerCtlRegister, kPowerCtl_Measure);
        if (rc != false) {
            //System.out.println("Did not connect to accelerometer");
            throw (new InstantiationException());
        }

        // Specify the accData format to read
        rc = write(kDataFormatRegister, kDataFormat_FullRes | range.value);
        if (rc != false) {
            //System.out.println("Did not connect to accelerometer");
            throw (new InstantiationException());
        }
        write(kIntMap, 0x7f);
        write(kFIFOCntl, 0x8f);
        write(0x2c, 0x0c);
        m_controlLoop = new java.util.Timer();
        m_controlLoop.schedule(new ChkTask(this), 0L, (long) (SAMPLE_TIME_ms));
        Calibrate();
        //     //System.out.println("accelerometer started");

        setActive(true);
    }

    public void Calibrate() {
        accMax.clear();
        accStill.clear();
        m_Calibrate = true;
        getAverages(); // clear averages
        write(OFSX, 0);
        write(OFSY, 0);
        setActive(true);
        write(kIntEnable, 0x80);
        while (mCount < 100) {
            //System.out.println("Calibrating:" + mCount);
            Timer.delay(.5);
        }

        accStill = getAverages();
        setActive(false);

        //System.out.println("STILL X=" + accStill.XAxis + "   Y=" + accStill.YAxis + "   Z=" + accStill.ZAxis);
//        write(OFSX, 0-(int)(accStill.XAxis/0.0156));
//        write(OFSY, 0-(int)(accStill.YAxis/0.0156));
        accMax.clear();
        setActive(true);
        while (mCount < 100) {
            //System.out.println("Calibrating2:" + mCount);
            Timer.delay(.5);
        }
        setActive(false);
        getAverages();
        accMax.XAxis *= 1.1;
        accMax.YAxis *= 1;
        accMax.ZAxis *= 1;
        //System.out.println("MAX X=" + accMax.XAxis + "   Y=" + accMax.YAxis + "   Z=" + accMax.ZAxis);
        m_Calibrate = false;
    }

    void getNewData() {
        boolean aborted = false;
        byte[] input = new byte[2];
        int count = 0;
        aborted = read(kIntSource, 1, input);
        if ((input[0] != (byte) 0x83) && (input[0] != (byte) 0x82)) {
            //System.out.println("!" + HexEncoding.hexEncode(input, 1));
        }

        //while no new accData availible wait
        while (((byte) (input[0] & (byte) 0x80) != (byte) 0x80)) {
            Timer.delay(.1);
            aborted = read(kIntSource, 1, input);
            count++;
        }
        if (count > 5) {
            //System.out.println("ADXL345_I2C_6DOF too long to get data:" + count);
        }
        read(kFIFOStatus, 1, input);
        int bytes = input[0] & 0x1f;

        if (bytes > 0) {
            //System.out.println("Activity:" + bytes);
        }
        do {
            readData();
        } while (bytes-- > 0);
//      //System.out.println("   count=" + mCount);
    }

    public boolean read(int registerAddress, int count, byte[] buffer) {
        int counter = 0;
        boolean aborted = super.read(registerAddress, count, buffer);
        while ((aborted == true) && (counter < 100)) {
            counter++;
            Timer.delay(.001);
            aborted = super.read(registerAddress, count, buffer);
        }
        if (counter > 5) {
            //System.out.println("!!!!!!!!!!Got to many aborts:" + counter);
        }
        return false;
    }

    /**
     * Constructor.
     *
     * @param slot The slot of the digital module that the sensor is plugged
     * into.
     * @param range The range (+ or -) that the accelerometer will measure.
     */
    /**
     * Get the acceleration of one axis in Gs.
     *
     * @param axis The axis to read from.
     * @return Acceleration of the ADXL345 in Gs.
     */
    public double getAcceleration(Axes axis) {
        byte[] rawAccel = new byte[2];
        boolean aborted = read(kDataRegister + axis.value, rawAccel.length, rawAccel);
        if (aborted) {
            return 0; // todo exception
        }


        // Sensor is little endian... swap bytes
        return accelFromBytes(rawAccel[0], rawAccel[1]);
    }

    private double accelFromBytes(byte first, byte second) {
        short tempLow = (short) (first & 0xff);
        short tempHigh = (short) ((second << 8) & 0xff00);
        return (tempLow | tempHigh) * kGsPerLSB;
    }
    /**
     * Get the acceleration of all axes in Gs.
     *
     * @return Acceleration measured on all axes of the ADXL345 in Gs.
     */
    int noXcount = 0;
    int noYcount = 0;

    private void readData() {
//        byte[] input = new byte[2];
        byte[] rawData = new byte[6];
        boolean aborted = false;

        if (mActive) {
            aborted = read(kDataRegister, 6, rawData);
            if (aborted) {
                //System.out.println("accel aborted");
                return;  // TODO exception
            }

            // Sensor is little endian... swap bytes

            accData.XAxis = accelFromBytes(rawData[0], rawData[1]);
            accData.YAxis = accelFromBytes(rawData[2], rawData[3]);
            accData.ZAxis = accelFromBytes(rawData[4], rawData[5]);
            
//               if(!m_Calibrate){
//                   //System.out.println(accData.XAxis + "\t" + accData.YAxis + "\t" + accData.ZAxis);
//               }else{

//            //System.out.print("X DATA=" + accData.XAxis);

            // modify by the calibration averages
            accData.subtract(accStill);
            if (m_Calibrate == false) {
                if (mActivityX == false) {
                    if (Math.abs(accData.XAxis) < accMax.XAxis) {
                        mActivityCountX = 0;
                        accData.XAxis = 0;
                        noXcount++;
                        // if we did not do enough then zero it out
                    } else {
//                        //System.out.println("X activity with " + accData.XAxis + " above " + accMax.XAxis + "noAct=" + noXcount);
                        noXcount = 0;
                        mActivityCountX++;
                        if (mActivityCountX > ActCount) {
                            mActivityX = true;
                            mActivityCountX = 0;
                            //System.out.println("XActivity on ");
                        } else {
                            accData.XAxis = 0;
                        }
                    }

                } else {
                    if (Math.abs(accData.XAxis) > accMax.XAxis) {
                        // This one is still active 
                        //System.out.println("X Still active " + accData.YAxis + " above " + accMax.YAxis);
                        mActivityCountX = 0;
                    } else {
                        //System.out.println("X not active ");
                        accData.XAxis = 0;
                        mActivityCountX++;
                        if (mActivityCountX > InActCount) {
                            mActivityX = false;
                            mActivityCountX = 0;
                            //System.out.println("XActivity off ");
                        }
                    }
                }
                if (mActivityY == false) {
                    if (Math.abs(accData.YAxis) < accMax.YAxis) {
                        mActivityCountY = 0;
                        accData.YAxis = 0;
                        noYcount++;
                        // if we did not do enough then zero it out
                    } else {
//                        //System.out.println("Y activity with " + accData.YAxis + " above " + accMax.XAxis + "noAct=" + noYcount);
                        noYcount = 0;
                        mActivityCountY++;
                        if (mActivityCountY > ActCount) {
                            mActivityY = true;
                            mActivityCountY = 0;
                            //System.out.println("YActivity on ");
                        } else {
                            accData.YAxis = 0;
                        }
                    }

                } else {
                    if (Math.abs(accData.YAxis) > accMax.YAxis) {
                        // This one is still active 
                        //System.out.println("Y Still active " + accData.YAxis + " above " + accMax.YAxis);
                        mActivityCountY = 0;
                    } else {
                        //System.out.println("Y not active ");
                        accData.YAxis = 0;
                        mActivityCountY++;
                        if (mActivityCountY > InActCount) {
                            mActivityY = false;
                            mActivityCountY = 0;
                            //System.out.println("YActivity off ");
                        }
                    }
                }

        }

       // accData.YAxis = 0;
        accData.ZAxis = 0;

        accTotal.add(accData);
        mCount++;

//            //System.out.println("  X(NEW)=" + accData.XAxis);
        if (m_Calibrate) {

            // generate calibration max readings
            if (Math.abs(accData.XAxis) > accMax.XAxis) {
                accMax.XAxis = Math.abs(accData.XAxis);
            }
            if (Math.abs(accData.YAxis) > accMax.YAxis) {
                accMax.YAxis = Math.abs(accData.YAxis);
            }
            if (Math.abs(accData.ZAxis) > accMax.ZAxis) {
                accMax.ZAxis = Math.abs(accData.ZAxis);
            }
        }
//            }
    }

    
        else{
            //System.out.println("NOT ACTIVE");
    }
    
}
public ADXL345_I2C_6DOF.AllAxes getAccelerations() {
        return accData;
    }

    public AllAxes getAverages() {
        AllAxes average;
        average = new AllAxes();
        // //System.out.println("COUNT=" + count);
        if (mCount == 0) {
            average.clear();
            return average;
        }
        average.XAxis = accTotal.XAxis / mCount;
        average.YAxis = accTotal.YAxis / mCount;
        average.ZAxis = accTotal.ZAxis / mCount;

        accTotal.clear();
        mCount = 0;
        return average;
    

}

    private class ChkTask extends TimerTask {

    private ADXL345_I2C_6DOF m_me;

    public ChkTask(ADXL345_I2C_6DOF me) {
        //System.out.println("ADXL345_I2C_6DOF chktsk:");
        if (me == null) {
            throw new NullPointerException("Given ADXL345_I2C_6DOF was null");
        }
        m_me = me;
    }

    public void run() {
        if (m_me.isActive()) {
            m_me.getNewData();
            
        }
    }
}
}
